#include "MOTOR.h"
#include "PID.h"
#include "math.h"
#include <stdlib.h>
/*************************	编码变量	*********************************/
volatile int Encode_L = 0,Encode_R = 0;

// 通过外部中断读取编码值，并判断AB相相位确定正反转
void GROUP1_IRQHandler(void)
{
		static uint32_t gpioA,gpioB;
    switch( DL_Interrupt_getPendingGroup(DL_INTERRUPT_GROUP_1) )
    {
      case GPIO_MULTIPLE_GPIOA_INT_IIDX:
					gpioA = DL_GPIO_getEnabledInterruptStatus(GPIOA,AB1_A1_PIN | AB1_B1_PIN);
					gpioB = DL_GPIO_getEnabledInterruptStatus(GPIOA,AB2_A2_PIN | AB2_B2_PIN);
					if((gpioA & AB1_A1_PIN) == AB1_A1_PIN)
					{
							DL_GPIO_clearInterruptStatus(GPIOA,AB1_A1_PIN);
							if(DL_GPIO_readPins(GPIOA,AB1_A1_PIN) != 0)
							{
									if(DL_GPIO_readPins(GPIOA,AB1_B1_PIN) != 0)
										Encode_L--;
									else
										Encode_L++;
							}
							else
							{
									if(DL_GPIO_readPins(GPIOA,AB1_B1_PIN) != 0)
										Encode_L--;
									else
										Encode_L++;
							}
					}
					if((gpioA & AB1_B1_PIN) == AB1_B1_PIN)
					{
							DL_GPIO_clearInterruptStatus(GPIOA,AB1_B1_PIN);
							if(DL_GPIO_readPins(GPIOA,AB1_B1_PIN) != 0)
							{
									if(DL_GPIO_readPins(GPIOA,AB1_A1_PIN)!=0)
										Encode_L++;
									else
										Encode_L--;
							}
							else
							{
									if(DL_GPIO_readPins(GPIOA,AB1_A1_PIN) != 0)
										Encode_L--;
									else
										Encode_L++;
							}
					}
					
					if((gpioB & AB2_A2_PIN) == AB2_A2_PIN)
					{
							DL_GPIO_clearInterruptStatus(GPIOA,AB2_A2_PIN);
							if(DL_GPIO_readPins(GPIOA,AB2_A2_PIN) != 0)
							{
									if(DL_GPIO_readPins(GPIOA,AB2_B2_PIN)!=0)
										Encode_R++;
									else
										Encode_R--;
							}
							else
							{
									if(DL_GPIO_readPins(GPIOA,AB2_B2_PIN) != 0)
										Encode_R--;
									else
										Encode_R++;
							}
					}
					if((gpioB & AB2_B2_PIN) == AB2_B2_PIN)
					{
							DL_GPIO_clearInterruptStatus(GPIOA,AB2_B2_PIN);
							if(DL_GPIO_readPins(GPIOA,AB2_B2_PIN) != 0)
							{
									if(DL_GPIO_readPins(GPIOA,AB2_A2_PIN)!=0)
										Encode_R--;
									else
										Encode_R++;
							}
							else
							{
									if(DL_GPIO_readPins(GPIOA,AB2_A2_PIN) != 0)
										Encode_R++;
									else
										Encode_R--;
							}
					}
      break;
			default:
				break;
    }
}

// 电机初始化:停转
void MOTOR_Init(void)
{
	Set_direction(S,S);
	DL_TimerG_setCaptureCompareValue(PWM_G7_INST,999,GPIO_PWM_G7_C0_IDX);
	DL_TimerG_setCaptureCompareValue(PWM_G7_INST,999,GPIO_PWM_G7_C1_IDX);
}

void MOTOR_Stop(void)
{
	Lpid.TargetValue = 0;
	Rpid.TargetValue = 0;
	Lpid.ActualValue = 0;
	Rpid.ActualValue = 0;
	Get_Encode(L);//这个比较关键，不清编码值，停下来之后位置环会补回去
	Get_Encode(R);
	Set_direction(S,S);
	DL_TimerG_setCaptureCompareValue(PWM_G7_INST,999,GPIO_PWM_G7_C0_IDX);
	DL_TimerG_setCaptureCompareValue(PWM_G7_INST,999,GPIO_PWM_G7_C1_IDX);
}

// 设置电机方向
void Set_direction(unsigned char Ldir,unsigned char Rdir)
{
	if(Ldir == Z){
    DL_GPIO_clearPins(GPIOB, MOTOR_AIN_1_PIN);
    DL_GPIO_setPins(GPIOB, MOTOR_AIN_2_PIN);
	}else if(Ldir == F){
    DL_GPIO_clearPins(GPIOB, MOTOR_AIN_2_PIN);
    DL_GPIO_setPins(GPIOB, MOTOR_AIN_1_PIN);
	} else {
    DL_GPIO_clearPins(GPIOB, MOTOR_AIN_2_PIN);
    DL_GPIO_clearPins(GPIOB, MOTOR_AIN_1_PIN);
	}
	if(Rdir == Z){
    DL_GPIO_clearPins(GPIOA, MOTOR_BIN_2_PIN);
    DL_GPIO_setPins(GPIOA, MOTOR_BIN_1_PIN);
	} else if(Rdir == F){
    DL_GPIO_clearPins(GPIOA, MOTOR_BIN_1_PIN);
    DL_GPIO_setPins(GPIOA, MOTOR_BIN_2_PIN);
	} else {
    DL_GPIO_clearPins(GPIOA, MOTOR_BIN_1_PIN);
    DL_GPIO_clearPins(GPIOA, MOTOR_BIN_2_PIN);
	}
}

// 设置电机目标转速
void MOTOR_Target_Speed(float lspeed,float rspeed)
{
	Lpid.TargetValue = lspeed;
	Rpid.TargetValue = rspeed;
}


void MOTOR_Duty_Speed(int lspeed,int rspeed)
{
	static unsigned char Ldir,Rdir;
	if(lspeed < 0){
		Ldir = F;
		if(lspeed < -999) lspeed=-999;
		DL_TimerG_setCaptureCompareValue(PWM_G7_INST,999+lspeed,GPIO_PWM_G7_C1_IDX);
	} else {
		Ldir = Z;
		if(lspeed > 999) lspeed=999;
		DL_TimerG_setCaptureCompareValue(PWM_G7_INST,999-lspeed,GPIO_PWM_G7_C1_IDX);
	}
	
	if(rspeed < 0){
		Rdir = F;
		if(rspeed < -999) rspeed=-999;
		DL_TimerG_setCaptureCompareValue(PWM_G7_INST,999+rspeed,GPIO_PWM_G7_C0_IDX);
	} else {
		Rdir = Z;
		if(rspeed > 999) rspeed=999;
		DL_TimerG_setCaptureCompareValue(PWM_G7_INST,999-rspeed,GPIO_PWM_G7_C0_IDX);
	}
	Set_direction(Ldir,Rdir);
}


// 获取编码值并清零
int Get_Encode(unsigned char LR)
{
	static int Encode = 0;
	if(LR == L){
		Encode = Encode_L;
		Encode_L = 0;
	} else {
		Encode = Encode_R;
		Encode_R = 0;
	}
	return Encode;
}

